#include "algoritmos.h"
#include <iostream>

using std::cout;
using std::endl;

Algoritmos::Algoritmos()
{
    //AL PASAR ALGORITMOS OPTIMIZAR CON HEXA
    velocidad[0] = 0;
    velocidad[1] = 0;
    posicion[0] = 0;
    posicion[1] = 0;
    aceleracion[0] = 0;
    aceleracion[1] = 0;
    offset = 0;
    muestrasCalibracion = 1024;
    muestrasFiltro = 64;
    index = 0;
}

void Algoritmos::integrar(int value)
{
    if(index == 2000) index = 0;
    value -= 512;
    double valor = value*(3.3/(1023*0.206));

    aceleracion[index+1] = valor;

    velocidad[index+1] = velocidad[index] + (( aceleracion[index+1] + aceleracion[index] ) * 0.01 / 2 );
    posicion[index+1] = posicion[index] + ((0.01 * ( velocidad[index+1] - velocidad[index] )) / 2) + velocidad[index]*0.01;

    index++;
}

void Algoritmos::calibrar(double *val)
{
    for(int i=0; i < muestrasCalibracion; i++)
    {
        offset += val[i];
    }

    offset = offset / muestrasCalibracion;
}

double* Algoritmos::filtrar(double *val, int size)
{
    for( int i=0; i<size; i += muestrasFiltro )
    {
        int count = 0;
        int value = 0;
        do{
            value += val[i+count];
            count++;
        }while(count != muestrasFiltro);

        value = value / muestrasFiltro;

        count = 0;
        do{
            array[i+count] = value - offset;
            count++;
        }while(count != muestrasFiltro);
    }

    return array;
}

int Algoritmos::getOffset()
{
    return this->offset;
}

void Algoritmos::setMuestrasCalibracion(int calibracion)
{
    this->muestrasCalibracion = calibracion;
}

void Algoritmos::setMuestrasFiltro(int filtro)
{
    this->muestrasFiltro = filtro;
}

double Algoritmos::getPosicion()
{
    return (posicion[index]);
}
